#include "../includes.h"


#define FREQ_PWM 100      //100Hz (10msec)

/* Get Time Offset */
OT_ TimeOffsetTable;
// updated at 98-06-22 11:45AM, by AHAS
OT_ TraceOffsetTable;

void Init_CTRLFUNC();
void PWM(void);

//kang void ChangeDACValue(int volt);

void InitOffsetTable(OT_ *);
int  GetTimeOffset(OT_ *, int);
void InsertTimeOffset(OT_ *,int,int);


void Init_CTRLFUNC()
{
    InitOffsetTable(&TimeOffsetTable);        // GPS counter
    InitOffsetTable(&TraceOffsetTable);       // Trace counter
}



void MakePWMInterval(double DAC_)
{
	int i;
    	if(OSC.kind == Rubidium_OSC){
		return;
	}

	CONTROL.HigherFraction = (int)(DAC_*100.0)-(((int)DAC_)*100);
	CONTROL.LowerFraction = FREQ_PWM - CONTROL.HigherFraction;

    // PWM
   	if(CONTROL.LowerFraction == 0)  CONTROL.LowerFraction = 1 ;
	if(CONTROL.HigherFraction == 0)  CONTROL.HigherFraction = 1 ;

	if( DAC_ < 5 ){		DAC_ = 5;}
	if( DAC_ > 65530){ 	DAC_ = 65530;}


}

void PWM(void)
{
//    OSC.kind = OCXO_OSC;
   OSC.kind = Rubidium_OSC;//sewon rb

	//kang ChangeDACValue((int )CONTROL.CurrentDAC);
	ChangeDACValue(fdFPGA, (int )CONTROL.CurrentDAC);
}



void InitOffsetTable(OT_ *Table)
{

    Table->InsertCount = 0;
    Table->ModInsertCount = 0;
}


/* ---------------------------------------------------------------------
File Name     : InsertTimeOffset
Last Updated  : 98-03-04 6:29
Function Name :
Description   : insert difference value into DiffCounter
                if same value exist, just plus hit counter
                else, save new value, and plus index
Parameters    : difference
Return Values :
--------------------------------------------------------------------- */
void InsertTimeOffset(OT_ *OffsetTable, int offset, int difference)
{
    int i,ModDiff;
    char FOUND;

    OffsetTable->Offset[OffsetTable->InsertCount] = offset;
    OffsetTable->Difference[OffsetTable->InsertCount++] = difference;

	if( (difference<2000) && (difference>-2000)){

        ModDiff = difference/128;
        FOUND = NO;
        for(i = 0 ; i < OffsetTable->ModInsertCount; i++) {
			if(ModDiff == OffsetTable->ModDifference[i]) {
                OffsetTable->Count[i]++;        /* if found just increase counter */
                FOUND = YES;
			}
		}
        if(FOUND == NO) {
            OffsetTable->ModDifference[OffsetTable->ModInsertCount] = ModDiff;  /* else add new value */
            OffsetTable->Count[OffsetTable->ModInsertCount++] = 0;
		}
    }
    
    //if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
    //	if(KInterval==Kinterval_STEP_1){
	//		sprintf((char*)MESG," ITO> DF:%d MDF:%d F:%d ", difference, ModDiff, FOUND); 
	//		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	//		
	//		for(i=0;i<OffsetTable->ModInsertCount;i++){
	//			sprintf((char*)MESG," [%d]%d,%d,%d,%d ", 
	//				i, OffsetTable->Offset[i], OffsetTable->Difference[i], 
	//				OffsetTable->ModDifference[i], OffsetTable->Count[i]); 
	//			TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	//		}
	//		TransmitMessage((BYTE*)"\r\n",PORT_CONTROL);
	//	}
	//}
}



int GetTimeOffset(OT_ *OffsetTable, int Cutoff_value)
{

    int i,j;
    int sum,counter;
    int difference,offset,CenterDiff,ModDiff;
    int Slope,Average;
	int NominalOffset;
	char FOUND;

    for(i = 0, j = 0, CenterDiff = 0; i < OffsetTable->ModInsertCount; i++) {
        if(j < OffsetTable->Count[i]) {
            j = OffsetTable->Count[i];

			CenterDiff = OffsetTable->ModDifference[i]*128;
		}
	}

    for(i = 0, sum = 0, counter = 0 ; i < OffsetTable->InsertCount; i++) {
        difference = CenterDiff - OffsetTable->Difference[i];
		if( (difference >= -Cutoff_value && difference <= Cutoff_value) ){
			sum += OffsetTable->Difference[i];
			counter++;
		}
	}
	if( counter ) {
		Slope = sum/counter;
	} else {
		Slope = 0;
	}

	if(slope_debug) {Slope = 0; counter = 0;}
	
	if(Slope == 0 && counter == 0){
		for(i = 0, sum = 0, counter = 0; i < OffsetTable->InsertCount; i++) {
			sum += OffsetTable->Offset[i];
			counter++;
		}
	}
	else{
		OffsetTable->ModInsertCount = 0;
	
		for(i = 0, NominalOffset = 0 ; i < OffsetTable->InsertCount; i++) {
	
			difference = NominalOffset - OffsetTable->Offset[i];
			OffsetTable->Difference[i] = difference;
	
			ModDiff = difference/128;
			FOUND = NO;
	
			for(j = 0 ; j < OffsetTable->ModInsertCount; j++) {
				if(ModDiff == OffsetTable->ModDifference[j]) {
					OffsetTable->Count[j]++;        /* if found just increase counter */
					FOUND = YES;
				}
			}
			if(FOUND == NO) {
				OffsetTable->ModDifference[OffsetTable->ModInsertCount] = ModDiff;  /* else add new value */
				OffsetTable->Count[OffsetTable->ModInsertCount++] = 0;
			}
			NominalOffset += Slope;
		}
	
	
	
		for(i = 0, j = 0, CenterDiff = 0; i < OffsetTable->ModInsertCount; i++) {
			if(j < OffsetTable->Count[i]) {
				j = OffsetTable->Count[i];

				CenterDiff = OffsetTable->ModDifference[i]*128;
			}
		}
	
	
		// Get Average Offset from CenterOffset
		for(i = 0, sum = 0, counter = 0; i < OffsetTable->InsertCount; i++) {
			difference = CenterDiff - OffsetTable->Difference[i];
			if( (difference >= -Cutoff_value && difference <= Cutoff_value) ){
				sum += OffsetTable->Offset[i];
				counter++;
			}
		}
	}

	if ( counter ) {
		Average = sum/counter;
	} else {
		Average = 0;
	}

	//if(Average == 0){
	//	if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
	//		sprintf((char*)MESG," GTO> SL:%d CN:%d I:%d MI:%d CD:%d ", 
	//			Slope, counter, OffsetTable->InsertCount, OffsetTable->ModInsertCount, CenterDiff); 
	//		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	//		
	//		if(KInterval==Kinterval_STEP_1 && control_mode==MODE_LOCK){
	//			for(i=0;i<OffsetTable->InsertCount;i++){
	//				sprintf((char*)MESG," [%d]%d,%d,%d,%d ", 
	//					i, OffsetTable->Offset[i], OffsetTable->Difference[i], 
	//					OffsetTable->ModDifference[i], OffsetTable->Count[i]); 
	//				TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	//			}
	//		}
	//		
	//		TransmitMessage((BYTE*)"\r\n",PORT_CONTROL);
	//	}
	//}
	
	OffsetTable->InsertCount = 0;
	OffsetTable->ModInsertCount = 0;

	return Average;
}



